#include "smallsatos.h"
#include "can_rx.h"
#include "ECAN.h"

OS_EVENT* can_new_message;

INT8U can_incoming_queue_name[] = "Incoming";
void* can_incoming_queue_content[CAN_INCOMING_QUEUE_SIZE];
OS_EVENT* can_incoming_queue;

OS_EVENT* get_can_incoming_queue(void)
{
	return can_incoming_queue;
}

OS_EVENT* get_can_new_message(void)
{
	return can_new_message;
}

void can_rx(void* pdata)
{
	INT16U err;

	/* Incoming buffers */
	static BYTE data[CAN_INCOMING_QUEUE_SIZE][8];
	static can_param_t param[CAN_INCOMING_QUEUE_SIZE];

    unsigned long id;
	BYTE dataLen;
	ECAN_RX_MSG_FLAGS flags;

	INT8U data_pos; /* First available buffer position */

	data_pos = 0;

    /* Initialise "New message" semaphore */
	can_new_message = OSSemCreate(0);

    /* Initialise the incoming queue */
	can_incoming_queue = OSQCreate(&can_incoming_queue_content, CAN_INCOMING_QUEUE_SIZE);
	OSEventNameSet(can_incoming_queue, can_incoming_queue_name, &err);

	for (;;)
	{
	    /* Wait for a new message interruption */
		OSSemPend(can_new_message, 0, &err);

		if (ECANReceiveMessage(&id, data[data_pos], &dataLen, &flags))
		{
		    /* Fill the message */
			param[data_pos].address = id;
			param[data_pos].data = data[data_pos];
			param[data_pos].length = dataLen;
			
			/* Put the message into the incoming queue */
			OSQPost(get_can_incoming_queue(), &param[data_pos]);

            /* Update the position of the first available buffer */
			data_pos = (data_pos + 1) % CAN_INCOMING_QUEUE_SIZE;
		} else {
			/* TODO: ERROR */
		}
		MCP2551_RS = 1; /* Message received: put the transceiver to sleep */
	}
}